Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Tracking an unknown number of objects

Goal

In highly dynamic scenarios with occlusions and multiple objects that have the same visual appearance, data association is a challenging and non-trivial task. This tutorial shows the tracking capabilities of wire by playing the game of cups.

Approach

Each cup in wire is tracked by means of a constant velocity Kalman filter. Each Kalman filter has a constant velocity motion model with a high process noise, since it is known that the constant velocity motion model is only a reasonable approximation for short time intervals and measurements provide valuable new information. In order to update the Kalman filters, the data association has to be solved. A multiple hypothesis-based approach is adopted in which all possible explanations from measurements to world model objects are considered:

• A measurement can originate from an object not present in the world model yet
• A measurement can represent a false positive
• A measurement can originate from an object already present in the world model

All possible explanations form a hypothesis tree and the most probable hypothesis is used as a world state estimate.

The process model and the process noise are defined in the file models/world_object_models.xml in wire_core package.

   1 <knowledge>
   2 
   3     <prior_new value="0.14" />
   4     <prior_existing value="0.14" />
   5     <prior_clutter value="0.72" />
   6 
   7     <object_class name="object"> 
   8     
   9         <behavior_model attribute="position" model="wire_state_estimators/PositionEstimator">
  10             <pnew type="uniform" dimensions="3" density="0.0001" />            
  11             <pclutter type="uniform" dimensions="3" density="0.0001" />
  12 
  13             <param name="max_acceleration" value="8" />                
  14             <param name="kalman_timeout" value="1" />                
  15             <param name="fixed_pdf_cov" value="0.008" />                
  16         </behavior_model>
  17 
  18         <behavior_model attribute="color" model="wire_state_estimators/DiscreteEstimator">
  19             <pnew type="discrete" domain_size="100" />            
  20             <pclutter type="discrete" domain_size="100" />
  21         </behavior_model> 
  22 
  23         <behavior_model attribute="class_label" model="wire_state_estimators/DiscreteEstimator">
  24             <pnew type="discrete" domain_size="100" />            
  25             <pclutter type="discrete" domain_size="100" />
  26         </behavior_model>
  27 
  28         <behavior_model attribute="shape" model="wire_state_estimators/DiscreteEstimator">
  29             <pnew type="discrete" domain_size="10" />            
  30             <pclutter type="discrete" domain_size="10" />
  31         </behavior_model>
  32 
  33     </object_class>
  34     
  35 </knowledge>

The Models XML File Explained

Now, let's break the launch xml down.

   7     <object_class name="object"> 

Here the models for the default object class are given. By adding multiple object_class elements, models can be made class specific.

   9         <behavior_model attribute="position" model="wire_state_estimators/PositionEstimator">
  10             <pnew type="uniform" dimensions="3" density="0.0001" />            
  11             <pclutter type="uniform" dimensions="3" density="0.0001" />
  12 
  13             <param name="max_acceleration" value="8" />                
  14             <param name="kalman_timeout" value="1" />                
  15             <param name="fixed_pdf_cov" value="0.008" />                
  16         </behavior_model>

The position will be estimated using a position estimator. This estimator, defined in the package [wire_state_estimators] is a multiple model estimator that combines (i) a Kalman filter with a constant velocity motion model with (ii) a fixed state with fixed uncertainty.

If updates follow each other relatively quickly, the Kalman filter is used to estimate the position. However, if no updates are received for kalman_timeout seconds, the Kalman filter makes place for a fixed state and uncertainty. This fixed state is defined by a Gaussian, the mean of which is based on the last estimated position, and the covariance is chosen to be fixed_pdf_cov.

Data

In order to be able to reproduce the result shown in the video above, make sure that you have cloned and compiled [[wire]:

$ git clone https://github.com/tue-robotics/wire.git
$ catkin_make

Download the bag-file (demo05.bag) and decompress it:

$ rosbag decompress demo05.bag

The bag file contains tfs, object detections and both rgb and depth images. The images are only included for ease of interpretation and inspection. These are not used by wire.

Reproducing the result

Start a ROS core:

$ roscore

and set the use_sim_time parameter to true:

$ rosparam set use_sim_time true

Then make sure the world_object_models.xml file in the models folder of the wire_core package is configured correctly. For reproducing the result, set the following behavior model for the position attribute for object_class object:

   9         <behavior_model attribute="position" model="wire_state_estimators/PositionEstimator">
  10             <pnew type="uniform" dimensions="3" density="0.0001" />            
  11             <pclutter type="uniform" dimensions="3" density="0.0001" />
  12 
  13             <param name="max_acceleration" value="8" />                
  14             <param name="kalman_timeout" value="1" />                
  15             <param name="fixed_pdf_cov" value="0.008" />                
  16         </behavior_model>

Now launch the world model:

$ roslaunch wire_core start.launch

In a new terminal, launch the visualization:

$ roslaunch wire_tutorials rviz_wire_kinetic.launch

Finally, play back the data:

$ rosbag play demo05.bag --clock

The results should be similar to the results shown in the video above.